/**
 * Authors:
 *    Kenneth Beck
 *    Erin Wirch
 *    Kevin Miller
 */
#include <iostream>
#include <libplayerc++/playerc++.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <string>

#include "AStar.h"
#include <GL/glut.h>
#include <fstream>

using namespace std;
using namespace PlayerCc;


#define METERS_PER_PIXEL 0.082
#define ROBOT_BOX 0.50

/*GLOBALS*/
static vector<vector<GridCoord> > *mainGrid;
static int window_width,window_height;
static bool good;
static Grid* theGrid;
static list<GridCoord*> path;
static char* ptsFile;

static PlayerClient    *robot;
static SonarProxy      *sp;
static Position2dProxy *pp;

/*PROTOTYPES*/
void findPath ( GridCoord *start );
void Follow( void );
double distanceBetween ( double x1, double y1, double x2, double y2 );


static void display() {
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   gluOrtho2D(	0,window_width,0,window_height );

   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();

   if( mainGrid != 0 ){
      glBegin( GL_POINTS );
      for( int x = 0; x < window_width; x++ ) {
         for( int y = 0; y < window_height; y++ ) {
            double val = (*mainGrid)[x][window_height-y-1].cVal;
            glColor3f( 1-val,1-val,1-val );
            glVertex2i(x,y);
         }
      }
      glEnd();
   }

   // Flush the pipeline
   glFlush();

   // Swap the buffers now that we're done drawing.
   glutSwapBuffers();
   good = true;
}

void redisplay() {
   if(good) glutPostRedisplay();
}

void* displayLoop(void* args) {
   while(1){
      //Have the robot follow the path
      Follow();
      //Refresh display
      redisplay();
      usleep(10);
   }
}

int main(int argc,char *argv[]){
   if( argc < 7 ){
      cout << "Usage: " << 
         argv[0] << " <raw_grid_file> <width> <height> <pts_file> <host> <port> " << endl;
      return 1;
   }
   char *iFile = argv[1];
   int width=atoi(argv[2]);
   int height=atoi(argv[3]);
   ptsFile = argv[4];
   int port = atoi(argv[6]);

   robot = new PlayerClient(argv[5], port); //may want to change this
   sp = new SonarProxy(robot, 0);
   pp = new Position2dProxy(robot, 0);
   pp->SetMotorEnable(1);
   pp->SetSpeed(0.1, 0.0);

   //parses the grid file from .RAW 
   theGrid = new Grid(Grid::parse_gridFile(iFile,width,height),METERS_PER_PIXEL,ROBOT_BOX);

   mainGrid = &theGrid->grid;

   //Set an initial start location
   GridCoord *start = (*theGrid)[DCoord(-678,-132)];

   //Generate the path!
   findPath( start );

   //set up display dimensions
   window_width=width;
   window_height=height;

   //start the display thread
   pthread_t thread_id;
   pthread_create(&thread_id,NULL,displayLoop,NULL);

   //set up GLUT stuff, create window.
   glutInit( &argc,argv );
   glutInitDisplayMode( GLUT_RGB | GLUT_DOUBLE );
   glutInitWindowPosition( 50,50 );
   glutInitWindowSize( window_width,window_height );
   glutCreateWindow( "Map" );



   // Callbacks
   glutDisplayFunc( display );

   glutMainLoop();

   return 0;
}//main



void findPath ( GridCoord *start )
{
   //clear the path
   path.clear();
   //open the points file
   ifstream ptsStream(ptsFile);

   //set up lists for path and points
   list<GridCoord*> points;


   //go through the points file, read in all points
   double x,y;
   while( ptsStream >> x >> y ){
      points.push_back(theGrid->get_gridCoord(Coord(x,y)));
   }


   //set up the Astar for path-finding
   AStar aStar(theGrid);

   while(points.size() > 0){
      GridCoord *goal = points.front();
      cout << "Going from: " << (*start) << " to: " << (*goal) << endl;

      list<GridCoord*> curPath = aStar.search(start,goal);
      start = points.front();
      points.pop_front();

      /*cout << "Before reduce: " << curPath.size() << endl;
        curPath = Grid::vertex_reduce(curPath);
        cout << "After reduce: " << curPath.size() << endl;*/

      while(!curPath.empty()){
         path.push_back(curPath.front());
         curPath.pop_front();
      }
      //Hand next points in path to AvoidObstacles
      // avoid.Follow( theGrid.get_worldCoord(goal).x, 
      //   theGrid.get_worldCoord(goal).y );   
   }

   list<GridCoord*>::iterator it;
   for(it=path.begin();it!=path.end();it++){
      (*theGrid)[(*it)->wPos]->cVal = 0.5;
      (*theGrid)[(*it)->wPos]->wVal = 0.5;
   }
   //grabfunction


}//findPath



void Follow( void )
{
   //Start measuring time
   time_t start_seconds, current_seconds;
   start_seconds = time(NULL);

   double turnrate, speed;
   double desired_angle;
   double constant_angle;
   double turnBy = 0.5;
   double direction = 1;

   constant_angle = 5;
   speed = 0.1;


   // Find next path point
   // Multiply by 0.02 to convert from pixels to meters
   double xVal, yVal;
   xVal = /*0.02 * */theGrid->get_worldCoord(path.back()).x;
   yVal = /*0.02 * */theGrid->get_worldCoord(path.back()).y;

   //xVal = /*0.02 * */theGrid->get_worldCoord(path.front()).x;
   //yVal = /*0.02 * */theGrid->get_worldCoord(path.front()).y;

   // read from the proxies
   robot->Read();
   double xCoord = pp->GetXPos() - 15.5;
   double yCoord = pp->GetYPos() + 12;
   double yaw = pp->GetYaw() + dtor(0);

   //Find distance to next point in path
   double delta = distanceBetween( xVal, yVal, xCoord, yCoord);
   cout << "Path: (" << xVal << ", " << yVal << ")" << endl;
   cout << "Distance to path point: " << delta << endl;


   current_seconds = (time(NULL) - start_seconds);

   //find & print distance that robot supposedly traveled
   //not to be trusted, based on time & speed, not actually distance...
   float distanceTraveled = speed * current_seconds;

   if ((sp->GetScan(0) < 0.25 || sp->GetScan(1) < 0.25 || sp->GetScan(2) < 0.25 ||
            sp->GetScan(3) < 0.25))
   {
      direction = -1;
      //DEBUGGING COMMENTS
      cout << "Uhoh, approaching obstacle on the left!\n";
      cout << "SONAR 0: \n" << sp->GetScan(0);
      cout << "SONAR 1: \n" << sp->GetScan(1);
      cout << "SONAR 2: \n" << sp->GetScan(2);
      cout << "SONAR 3: \n" << sp->GetScan(3);
      if ((sp->GetScan(0) < 0.15 || sp->GetScan(1) < 0.15 || sp->GetScan(2) <
               0.15 || sp->GetScan(3) < 0.15))
      {
         direction  = 0;
         speed = -0.1;
         cout << "Too close to something on the left-- backing up!\n";
      }//if
   } else if ((sp->GetScan(4) < 0.25 || sp->GetScan(5) < 0.25 || sp->GetScan(6) <
            0.25 || sp->GetScan(7) < 0.25)) {
      direction = 1;
      //DEBUGGING COMMENTS
      cout << "Uhoh, approaching obstacle on the right!\n";
      cout << "SONAR 4: \n" << sp->GetScan(4);
      cout << "SONAR 5: \n" << sp->GetScan(5);
      cout << "SONAR 6: \n" << sp->GetScan(6);
      cout << "SONAR 7: \n" << sp->GetScan(7);
      if ((sp->GetScan(4) < 0.15 || sp->GetScan(5) < 0.15 || sp->GetScan(6) <
               0.15 || sp->GetScan(7) < 0.15))
      {
         direction  = 0;
         speed = -0.1;
         //DEBUGGING COMMENTS
         cout << "Too close to something on the right-- backing up!\n";
      }//else if

   } else {
      if ( delta > .38 ) //if we're too far from the goal
      {
         desired_angle = atan2 ( ( yVal - yCoord), 
               ( xVal - xCoord ) ); 
         if (rtod(desired_angle) > 180 )
         {
            desired_angle = dtor(rtod(desired_angle) - 360);
            cout<<"Convert desired angle to Pi to -PI\n";
         }//if

         double error = desired_angle - yaw;
         if (rtod(error) > 180 )
         {
            error = dtor (rtod(error)  - 360);
         }//if
         else if (rtod(error) < -180)
         {
            error = dtor (rtod(error) + 360 );
         }//else  if
         cout<<"Desired angle: "<<rtod(desired_angle)<<endl;
         cout<<"Yaw Degrees : "<<rtod(yaw)<<endl;
         cout << "error: " << rtod(error) << endl;
         if (abs(error) < 0.1 )
         {
            direction = 0;
            cout<<"Setting ANGLE TO zero\n";
         }//if
         else if (error > 0 )
         {
            direction  = 1;
         }//else if  
         else
         {
            direction = -1;
         }//else


      }//if
      else //we're close enough, no more turning
      {
         //We need to get the next point now that we've reached the
         //old goal in tha path.
         path.pop_back();
         desired_angle = 0;
         cout<<"Reached a goal! Onto next point in path.\n";
      }//else
   }//else

   turnrate = turnBy * direction; // turn d_a per sec.
   /*
   cout<<"Turnrate: "<<turnrate<<"\n";
   cout<<"aTan: "<<desired_angle + pp.GetYaw() <<"\n";
   cout<<"Direciton: "<<direction<<endl;
   */
   cout<<"Direction: "<<direction<<endl;
   pp->SetSpeed(speed, turnrate);

   cout << "SPEED: " << speed << "\n";



   printf("X %5.3f y %5.3f theta %5.3f\n",
         xCoord,yCoord,yaw);

   //}//while
   //}//for
}//FollowWall



double distanceBetween ( double x1, double y1, double x2, double y2 )
{
   double dx = x2 - x1;
   double dy = y2 - y1;

   return sqrt( dx*dx + dy*dy );

}//distanceBetween
